#include "lib/MarkerViz.hpp"


MarkerViz::MarkerViz(const std::string &name) : Node(name){

    marker.header.stamp = rclcpp::Clock().now();
    marker.ns = "box_filter";
    marker.id = 0;
    marker.type = visualization_msgs::msg::Marker::CUBE;
    marker.action = visualization_msgs::msg::Marker::ADD;

    config = YAML::LoadFile(YAML_FILE_PATH);
    std::string transform_to_robot_frame = config["transform_to_robot_frame"].as<std::string>();
    marker.header.frame_id = config["frame_id"]["robot"].as<std::string>();

    min_x = config["box_size"]["min"]["x"].as<double>();
    min_y = config["box_size"]["min"]["y"].as<double>();
    min_z = config["box_size"]["min"]["z"].as<double>();
    max_x = config["box_size"]["max"]["x"].as<double>();
    max_y = config["box_size"]["max"]["y"].as<double>();
    max_z = config["box_size"]["max"]["z"].as<double>();
    use_marker = config["box_filter"]["visualize_box"].as<std::string>();

    std::string marker_name = config["output_topic"]["marker"].as<std::string>();
    if (use_marker=="true") {
        loadBoxDimensions();
        pub_marker = this
                ->create_publisher<visualization_msgs::msg::Marker>(name, 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MarkerViz::visualize, this));
    }
}

void MarkerViz::loadBoxDimensions() {
    // 设定盒子中心为 (0, 0, 0)
    box.minPoint = { min_x, min_y, min_z };
    box.maxPoint = { max_x, max_y, max_z };

    // 设置 marker 的位置为盒子中心 (0, 0, 0)
    marker.pose.position.x = (min_x + max_x) / 2;
    marker.pose.position.y = (min_y + max_y) / 2;
    marker.pose.position.z = (min_z + max_z) / 2;

    // 盒子的尺寸
    marker.scale.x = max_x - min_x;
    marker.scale.y = max_y - min_y;
    marker.scale.z = max_z - min_z;

    // 设置方向为默认值
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // 设置颜色
    marker.color.a = 0.5;
    marker.color.r = 1.0;
    marker.color.g = 0.75;
    marker.color.b = 0.79;
}

void MarkerViz::visualize() {
    pub_marker->publish(marker); // 发布 marker 消息
}

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MarkerViz>("marker_viz");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}